﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;


namespace Integrador
{

    public class Dinamica
    {

        #region Delecgados


        #endregion

        #region Eventos


        #endregion

        #region Atributos
        //estado actual
        private Estado estadoActual;
        //estado inicial
        private Estado estadoInicial;
        //gravedad
        private double g = 9.8;
        //cr supuesto radio de la unidad central del quad(en m)
        private double cR = 0.07;
        //longitud de las helices
        private double pR = 0.1;
        //masa del quad
        private double M = 0.7;
        //masa de la helice y engranage
        private double m = 0.065;
        //distancia del motor al centro del quad
        private double l = 0.23;
        //masa total
        private double M_Total;
        //constante del aire
        private static double rho = 1.293;
        private double Ac;
        private double Ar;
        //constantes del momento
        private double k1 = 1;
        private double k2 = 0.01;
        //momentos de inercia
        private double Jx, Jy, Jz, a_x, a_y, a_z;
        //coeficientes de arrastre
        private double Cx = 1.3, Cy = 1.3, Cz = 1.3, Cd = 0.052;


        //lista con todos los estados
        public ColeccionEstados estados { get; set; }

        public List<double> tiempo { get; set; }

        //torque
        private double T_phi, T_theta, T_psi;



        // Matriz de paso velocidad angular

        private double[,] M_vel;

        private double Throttle;
        private double[,] M_rot;
        private double[,] M_rotT;

        #endregion

        #region Constructores

        public Dinamica(double[] rot, Estado estadoInicial)
        {
            //condiciones iniciales
            this.estadoInicial = estadoInicial;
            this.estadoActual = new Estado();
            double[] Rot = rot;
            this.estados = new ColeccionEstados();
            this.tiempo = new List<double>();

            this.M_vel = new double[3, 3];
            this.M_Total = M + 4 * m;
            this.Ac = Math.PI * Math.Pow(this.cR, 2);
            this.Ar = Math.PI * Math.Pow(this.pR, 2);
            this.Jx = (2 / 5) * this.M * Math.Pow(this.cR, 2) + 2 * this.m * Math.Pow(this.l, 2);
            this.Jy = (2 / 5) * this.M * Math.Pow(this.cR, 2) + 2 * this.m * Math.Pow(this.l, 2);
            this.Jz = (2 / 5) * this.M * Math.Pow(this.cR, 2) + 4 * this.m * Math.Pow(this.l, 2);
            this.a_x = (this.Jy - this.Jz) / this.Jx;
            this.a_y = (this.Jz - this.Jx) / this.Jy;
            this.a_z = (this.Jx - this.Jy) / this.Jz;
            //llamar al integrador
            this.Integrador(10, 100, 20, estadoInicial, Rot);

        }

        #endregion
        /// <summary>
        /// integrador
        /// </summary>
        /// <param name="a">t inicial</param>
        /// <param name="b">t final</param>
        /// <param name="N">N numero de pasos</param>
        /// <param name="ini">estado inicial</param>
        /// <param name="Rot">estado de los rotores</param>
        public void Integrador(double a, double b, double N, Estado ini, double[] Rot)
        {
            Estado rk1, rk2, rk3, rk4;
            //tamaño del paso
            double h = (b - a) / N;
            double t = a;
            this.estadoActual = this.estadoInicial;
            for (int i = 1; i <= N + 1; i++)
            {
                //k1 = h*quad_dynamics(t(i), w(:,i),Rot);
                rk1 = multiplica(DameEstadoSiguiente(this.estadoActual, Rot), h);
                //k2 = h*quad_dynamics(t(i)+h/2, w(:,i)+0.5*k1, Rot);
                t = t + h / 2;
                rk2 = multiplica(DameEstadoSiguiente(suma(this.estadoActual, multiplica(rk1, 0.5)), Rot), h);
                // k3 = h*quad_dynamics(t(i)+h/2, w(:,i)+0.5*k2, Rot); 
                t = t + h / 2;
                rk3 = multiplica(DameEstadoSiguiente(suma(this.estadoActual, multiplica(rk2, 0.5)), Rot), h);
                //k4 = h*quad_dynamics(t(i)+h, w(:,i)+k3, Rot);
                rk4 = multiplica(DameEstadoSiguiente(suma(this.estadoActual, rk3), Rot), h);
                // w(:,i+1) = w(:,i) + (k1 + 2*k2 + 2*k3 + k4)/6;
                Estado es = suma(this.estadoActual, multiplica(suma(suma(rk1, multiplica(rk2, 2)), suma(multiplica(rk3, 2), rk4)), 0.1666666));
                this.Limitaciones(es);
                this.estadoActual = es;
                this.estados.Add(this.estadoActual);
                this.tiempo.Add(t);
                t = a + i * h;

            }
            this.estados.ForEach(x => System.Console.Write(x.ToString()));
        }
        public Estado IntegraPaso(ref double tiempoTotal, double tamanoPaso, double[] Rot)
        {
            Estado rk1, rk2, rk3, rk4;
            if (tiempoTotal == 0)
            {
                this.estadoActual = this.estadoInicial;
            }
            rk1 = multiplica(DameEstadoSiguiente(this.estadoActual, Rot), tamanoPaso);
            rk2 = multiplica(DameEstadoSiguiente(suma(this.estadoActual, multiplica(rk1, 0.5)), Rot), tamanoPaso);
            rk3 = multiplica(DameEstadoSiguiente(suma(this.estadoActual, multiplica(rk2, 0.5)), Rot), tamanoPaso);
            rk4 = multiplica(DameEstadoSiguiente(suma(this.estadoActual, rk3), Rot), tamanoPaso);
            Estado es = suma(this.estadoActual, multiplica(suma(suma(rk1, multiplica(rk2, 2)), suma(multiplica(rk3, 2), rk4)), 0.1666666));
            this.Limitaciones(es);
            this.estadoActual = es;
            //actualizamos el tiempo en el tick del reloj
            tiempoTotal = tiempoTotal + tamanoPaso;
            tiempoTotal = Math.Round(tiempoTotal, 2);
            return es;
        }

        public Estado DameEstadoSiguiente(Estado estado, double[] Rot)
        {
            Estado estadoSiguiente = new Estado();


            T_phi = l * k1 * (-Rot[1] + Rot[3]);
            T_theta = l * k1 * (Rot[0] - Rot[2]);
            T_psi = k2 * (Rot[0] - Rot[1] + Rot[2] - Rot[3]);

            // Throttle
            Throttle = k1 * (Rot[0] + Rot[1] + Rot[2] + Rot[3]);

            //Matriz de paso de vel_angular S.inercial a S.vehiculo
            M_vel = new double[3, 3];

            M_vel[0, 0] = 1;
            M_vel[0, 1] = Math.Sin(estado.phi) * Math.Tan(estado.theta);
            M_vel[0, 2] = Math.Cos(estado.phi) * Math.Tan(estado.theta);
            M_vel[1, 0] = 0;
            M_vel[1, 1] = Math.Cos(estado.phi);
            M_vel[1, 2] = -Math.Sin(estado.phi);
            M_vel[2, 0] = 0;
            M_vel[2, 1] = Math.Sin(estado.phi) * (1 / Math.Cos(estado.theta));
            M_vel[2, 2] = Math.Cos(estado.phi) * (1 / Math.Cos(estado.theta));


            M_rot = new double[3, 3];
            M_rot[0, 0] = Math.Cos(estado.psi) * Math.Cos(estado.theta);
            M_rot[0, 1] = Math.Sin(estado.psi) * Math.Cos(estado.theta);
            M_rot[0, 2] = -Math.Sin(estado.theta);
            M_rot[1, 0] = -Math.Sin(estado.psi) * Math.Cos(estado.phi) + Math.Cos(estado.psi) * Math.Sin(estado.theta) * Math.Sin(estado.phi);
            M_rot[1, 1] = Math.Cos(estado.psi) * Math.Cos(estado.phi) + Math.Sin(estado.psi) * Math.Sin(estado.theta) * Math.Sin(estado.phi);
            M_rot[1, 2] = Math.Cos(estado.theta) * Math.Sin(estado.phi);
            M_rot[2, 0] = Math.Sin(estado.psi) * Math.Sin(estado.phi) + Math.Cos(estado.psi) * Math.Sin(estado.theta) * Math.Cos(estado.phi);
            M_rot[2, 1] = -Math.Cos(estado.psi) * Math.Sin(estado.phi) + Math.Sin(estado.psi) * Math.Sin(estado.theta) * Math.Cos(estado.phi);
            M_rot[2, 2] = Math.Cos(estado.theta) * Math.Cos(estado.phi);
            M_rotT = transponer(M_rot);

            double fr_roll = Cz * Ar * rho * estado.P * Math.Abs(estado.P) * Math.Pow(l, 2); //%Cz*Ar*rho*P*abs(P)*l^2;   % Roll friction
            double fr_pitch = Cz * Ar * rho * estado.Q * Math.Abs(estado.Q) * Math.Pow(l, 2); //%Cz*Ar*rho*Q*abs(Q)*l^2;   % Pitch friction

            double fr_x = (Cx * rho * Ac * estado.U * Math.Abs(estado.U)) / 2;//%(Cx*rho*Ac*U*abs(U))/2;
            double fr_y = (Cy * rho * Ac * estado.V * Math.Abs(estado.V)) / 2;//%(Cy*rho*Ac*V*abs(V))/2;
            double fr_z = (Cz * rho * Ac * estado.W * Math.Abs(estado.W)) / 2;//%(Cz*rho*Ac*W*abs(W))/2;

            estadoSiguiente.phi = M_vel[0, 0] * estado.P + M_vel[0, 1] * estado.Q + M_vel[0, 2] * estado.R;   //dot_phi
            estadoSiguiente.theta = M_vel[1, 0] * estado.P + M_vel[1, 1] * estado.Q + M_vel[1, 2] * estado.R;   //dot_theta
            estadoSiguiente.psi = M_vel[2, 0] * estado.P + M_vel[2, 1] * estado.Q + M_vel[2, 2] * estado.R;   //dot_psi

            estadoSiguiente.P = (T_phi - fr_roll) / Jx + a_x * estado.Q * estado.R;       //dot_P
            estadoSiguiente.Q = (T_theta - fr_pitch) / Jy + a_y * estado.P * estado.R;    //dot_Q
            estadoSiguiente.R = T_psi / Jz + a_z * estado.P * estado.Q;                   //dot_R

            // [dot_X dot_Y dot_Z]=M_rot*[U V W]
            estadoSiguiente.X = M_rotT[0, 0] * estado.U + M_rotT[0, 1] * estado.V + M_rotT[0, 2] * estado.W;   //dot_X
            estadoSiguiente.Y = M_rotT[1, 0] * estado.U + M_rotT[1, 1] * estado.V + M_rotT[1, 2] * estado.W;   //dot_Y
            estadoSiguiente.Z = M_rotT[2, 0] * estado.U + M_rotT[2, 1] * estado.V + M_rotT[2, 2] * estado.W;   //dot_Z

            estadoSiguiente.U = estado.R * estado.V - estado.Q * estado.W + M_rotT[2, 0] * g - fr_x / M_Total;              //dot_U
            estadoSiguiente.V = estado.P * estado.W - estado.R * estado.U + M_rotT[2, 1] * g - fr_y / M_Total;              //dot_V
            estadoSiguiente.W = estado.Q * estado.U - estado.P * estado.V - M_rotT[2, 2] * g + (Throttle - fr_z) / M_Total; //dot_W



            return estadoSiguiente;
        }

        public void Limitaciones(Estado estado)
        {
            if (estado.Z < 0)
            {
                estado.P = 0;
                estado.Q = 0;
                estado.R = 0;
                estado.Z = 0;
                estado.U = 0;
                estado.V = 0;
                estado.W = 0;
            }
            estado.phi = estado.phi - Math.Truncate(estado.phi / (2 * Math.PI)) * 2 * Math.PI;
            estado.theta = estado.theta - Math.Truncate(estado.theta / (2 * Math.PI)) * 2 * Math.PI;
            estado.psi = estado.psi - Math.Truncate(estado.psi / (2 * Math.PI)) * 2 * Math.PI;

            //        %out(2) = Pitch E[-pi/2,pi/2]
            //if (out(2) > (pi/2))
            //    out(2) = pi - out(2);
            //    out(1) = out(1) + pi;
            //    out(3) = out(3) + pi;
            //elseif (out(2) < (-pi/2))
            //    out(2) = -pi - out(2);
            //    out(1) = out(1) + pi;
            //    out(3) = out(3) + pi;
            //end
            if (estado.theta > (Math.PI / 2))
            {
                estado.theta = Math.PI - estado.theta;
                estado.phi = estado.phi + Math.PI;
                estado.psi = estado.psi + Math.PI;
            }
            else if (estado.theta < (-Math.PI / 2))
            {
                estado.theta = -Math.PI - estado.theta;
                estado.phi = estado.phi + Math.PI;
                estado.psi = estado.psi + Math.PI;
            }

            //%out(1) = Roll E[-pi,pi]
            //if (out(1) > pi)
            //    out(1) = out(1) - 2*pi; 
            //elseif (out(1) < -pi)
            //    out(1) = out(1) + 2*pi;
            //end
            if (estado.phi > Math.PI)
            {
                estado.phi = estado.phi - 2 * Math.PI;
            }
            else if (estado.phi < -Math.PI)
            {
                estado.phi = estado.phi + 2 * Math.PI;
            }

            //%out(3) = Yaw E[-pi,pi]
            //if (out(3) > pi)
            //    out(3) = out(3) - 2*pi; 
            //elseif (out(3) < -pi)
            //    out(3) = out(3) + 2*pi;
            //end
            if (estado.psi > Math.PI)
            {
                estado.psi = estado.psi - 2 * Math.PI;
            }
            else if (estado.psi < -Math.PI)
            {
                estado.psi = estado.psi + 2 * Math.PI;
            }
        }


        private double[,] transponer(double[,] original)
        {
            double[,] transpuesta = new double[3, 3];
            for (int i = 0; i < original.GetLength(0); i++)
            {
                for (int j = 0; j < original.GetLength(1); j++)
                {
                    transpuesta[i, j] = original[j, i];
                }
            }
            return transpuesta;
        }

        public Estado multiplica(Estado estado, double constante)
        {
            Estado nuevoEstado = new Estado();
            nuevoEstado.P = estado.P * constante;
            nuevoEstado.phi = estado.phi * constante;
            nuevoEstado.psi = estado.psi * constante;
            nuevoEstado.Q = estado.Q * constante;
            nuevoEstado.R = estado.R * constante;
            nuevoEstado.theta = estado.theta * constante;
            nuevoEstado.U = estado.U * constante;
            nuevoEstado.V = estado.V * constante;
            nuevoEstado.W = estado.W * constante;
            nuevoEstado.X = estado.X * constante;
            nuevoEstado.Y = estado.Y * constante;
            nuevoEstado.Z = estado.Z * constante;
            return nuevoEstado;
        }
        public Estado divide(Estado estado, double constante)
        {
            Estado nuevoEstado = new Estado();
            nuevoEstado.P = estado.P / constante;
            nuevoEstado.phi = estado.phi / constante;
            nuevoEstado.psi = estado.psi / constante;
            nuevoEstado.Q = estado.Q / constante;
            nuevoEstado.R = estado.R / constante;
            nuevoEstado.theta = estado.theta / constante;
            nuevoEstado.U = estado.U / constante;
            nuevoEstado.V = estado.V / constante;
            nuevoEstado.W = estado.W / constante;
            nuevoEstado.X = estado.X / constante;
            nuevoEstado.Y = estado.Y / constante;
            nuevoEstado.Z = estado.Z / constante;
            return nuevoEstado;
        }

        public Estado suma(Estado estado, Estado estado2)
        {
            Estado nuevoEstado = new Estado();
            nuevoEstado.P = estado.P + estado2.P;
            nuevoEstado.phi = estado.phi + estado2.phi;
            nuevoEstado.psi = estado.psi + estado2.psi;
            nuevoEstado.Q = estado.Q + estado2.Q;
            nuevoEstado.R = estado.R + estado2.R;
            nuevoEstado.theta = estado.theta + estado2.theta;
            nuevoEstado.U = estado.U + estado2.U;
            nuevoEstado.V = estado.V + estado2.V;
            nuevoEstado.W = estado.W + estado2.W;
            nuevoEstado.X = estado.X + estado2.X;
            nuevoEstado.Y = estado.Y + estado2.Y;
            nuevoEstado.Z = estado.Z + estado2.Z;
            return nuevoEstado;
        }
        public Estado sumaConstante(Estado estado, double constante)
        {
            Estado nuevoEstado = new Estado();
            nuevoEstado.P = estado.P + constante;
            nuevoEstado.phi = estado.phi + constante;
            nuevoEstado.psi = estado.psi + constante;
            nuevoEstado.Q = estado.Q + constante;
            nuevoEstado.R = estado.R + constante;
            nuevoEstado.theta = estado.theta + constante;
            nuevoEstado.U = estado.U + constante;
            nuevoEstado.V = estado.V + constante;
            nuevoEstado.W = estado.W + constante;
            nuevoEstado.X = estado.X + constante;
            nuevoEstado.Y = estado.Y + constante;
            nuevoEstado.Z = estado.Z + constante;
            return nuevoEstado;
        }
        public Estado restaConstante(double constante, Estado estado)
        {
            Estado nuevoEstado = new Estado();
            nuevoEstado.P = constante - estado.P;
            nuevoEstado.phi = constante - estado.phi;
            nuevoEstado.psi = constante - estado.psi;
            nuevoEstado.Q = constante - estado.Q;
            nuevoEstado.R = constante - estado.R;
            nuevoEstado.theta = constante - estado.theta;
            nuevoEstado.U = constante - estado.U;
            nuevoEstado.V = constante - estado.V;
            nuevoEstado.W = constante - estado.W;
            nuevoEstado.X = constante - estado.X;
            nuevoEstado.Y = constante - estado.Y;
            nuevoEstado.Z = constante - estado.Z;
            return nuevoEstado;
        }
        public Estado resta(Estado estado, Estado estado2)
        {
            Estado nuevoEstado = new Estado();
            nuevoEstado.P = estado.P - estado2.P;
            nuevoEstado.phi = estado.phi - estado2.phi;
            nuevoEstado.psi = estado.psi - estado2.psi;
            nuevoEstado.Q = estado.Q - estado2.Q;
            nuevoEstado.R = estado.R - estado2.R;
            nuevoEstado.theta = estado.theta - estado2.theta;
            nuevoEstado.U = estado.U - estado2.U;
            nuevoEstado.V = estado.V - estado2.V;
            nuevoEstado.W = estado.W - estado2.W;
            nuevoEstado.X = estado.X - estado2.X;
            nuevoEstado.Y = estado.Y - estado2.Y;
            nuevoEstado.Z = estado.Z - estado2.Z;
            return nuevoEstado;
        }

    }
}